/*
 *
 * Copyright (C) 2014
 * Julio Jarquin, Gustavo Arechavaleta <garechav@cinvestav.edu.mx>
 * CINVESTAV - Saltillo Campus
 *
 * This file is part of HRLocomotion-1.0
 * HRLocomotion-1.0 free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * HRLocomotion-1.0 is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.

 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
 * 02110-1301  USA
 */

#include "HRLocomotion/robot_state_dep_.h"

namespace hrLocomotion{

RobotStateDep::RobotStateDep(){
    x_k.setZero(3,1);
    y_k.setZero(3,1);
    theta_k.setZero(3,1);

    augState.resize(9,1);
    augState << x_k, y_k, theta_k;
}

RobotStateDep::RobotStateDep(const RobotStateDep & state){
}


RobotStateDep& RobotStateDep::operator= (const RobotStateDep& state){
        x_k = state.x_k;
        y_k = state.y_k;
        theta_k = state.theta_k;

        augState = x_k , y_k , theta_k;

        return *this;

}

std::ostream& operator<< (std::ostream& os, const RobotStateDep& state){
    os << state.augState;
    return os;
}

void RobotStateDep::init(){
    x_k.setZero(3,1);
    y_k.setZero(3,1);
    theta_k.setZero(3,1);

    augState.resize(9,1);
    augState << x_k, y_k, theta_k;
}

void RobotStateDep::init(VectorXr x, VectorXr y){
    x_k = x;
    y_k = y;
    theta_k.setZero(3,1);
    augState.resize(9,1);
    augState << x_k, y_k, theta_k;
}

void RobotStateDep::init(VectorXr x, VectorXr y, VectorXr theta){
    x_k = x ;
    y_k = y;
    theta_k = theta;

    augState.resize(9,1);
    augState << x_k, y_k, theta_k;
}

}
